#define _GNU_SOURCE
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
#include <string.h>
#include <strings.h>
#include <fcntl.h>
#include <termios.h>
#include <time.h>
#include <sys/time.h>
#include <unistd.h>
#include "../logServer/clogger.h"
#include "syscore.h"
#include "apis.h"
//#include "gpsd.h"
//#include "gpsdclient.h"
#include "gpsctrl.h"
#include "modules.h"
#include "common.h"
#include <math.h>
#define GPS_RECV_CMD_MAX_BUF                       128 //the buffer size
#define RMC_MAX_BUF                                16  //the buffer size of rmc field

#define GPSLogDebug(fmt, args...) CLogger(MODULE_GPS, __FILE__, __LINE__, LOG_DEBUG, fmt, ##args);
#define GPSLogInfo(fmt, args...) CLogger(MODULE_GPS, __FILE__, __LINE__, LOG_INFO, fmt, ##args);
#define GPSLogNotice(fmt, args...) CLogger(MODULE_GPS, __FILE__, __LINE__, LOG_NOTICE, fmt, ##args);
#define GPSLogWarning(fmt, args...) CLogger(MODULE_GPS, __FILE__, __LINE__, LOG_WARNING, fmt, ##args);
#define GPSLogError(fmt, args...) CLogger(MODULE_GPS, __FILE__, __LINE__, LOG_ERR, fmt, ##args);
#define GPSLogCrit(fmt, args...) CLogger(MODULE_GPS, __FILE__, __LINE__, LOG_CRIT, fmt, ##args);
#define GPSLogAlert(fmt, args...) CLogger(MODULE_GPS, __FILE__, __LINE__, LOG_ALERT, fmt, ##args);
#define GPSLogEmerg(fmt, args...) CLogger(MODULE_GPS, __FILE__, __LINE__, LOG_EMERG, fmt, ##args);

bool stop_gpsprocess = false;
int gps_flag=0;
static bool reportGps = false;
MODREG modGpsInfo = {MODULE_GPS, TRUE, TRUE, 1000,NULL};	// 1s

#define GPS_TIMER_INTERVAL 10000
void *gpsTimer = NULL; 
bool gpsStarted = FALSE;

#define GPS_HB_INTERVAL 5000
void *gpsheartBeat = NULL;

bool gps_device_present = false;
int gps_debug_level = DEBUG_NONE;

#define GPS_RETRY_MAX  5
int gps_retry_num = 0;
/* enable logging level on gpsctrl if you wantto check GPSD JSON PARSing results */
// gps_debug_level = DEBUG_CALLS;

#define GPS_NODATA_THRESHOLD  5
int gps_continue_nodata_cnt = 0;

static const char zGpsdSource[] = "127.0.0.1:2947";

static struct gps_data_t gpsdata;

void GPS_getStatus(U32 *status, GPSPOSITION* pos);
double GPS_getDistance(GPSPOSITION* pos1,GPSPOSITION* pos2);
ERROR_CODE GPS_decodeGPSPosition(void *h, U8* paraValue, U32 paraLength );
ERROR_CODE GPS_decodeDistance(void *h, U8* paraValue, U32 paraLength );


BOOL expectedPosition_set = FALSE;
U32 gps_alert_threshold = 50;
GPSPOSITION currentPosition = {0};
GPSPOSITION expectedPosition = {0};

#define GPS_ONLINE 1
#define GPS_OFFLINE 0
U32 gpsStatus = GPS_OFFLINE;

typedef struct tagGPSPOSPARA {
	U32 setmask;
	GPSPOSITION pos;
} GPSPOSPARA;

PARADECODERMAP gpsPostionParaTab[] = {
        {PARA_TYPE_GPS_COORDINATE,GPS_decodeGPSPosition}
}; 

PARADECODERMAP gpsDistanceTab[] = {
        {PARA_TYPE_GPS_DISTANCE,GPS_decodeDistance}
}; 

void reportGpsHeartBeat(void *h)
{
    U32 rc = PARA_VALUE_SYSMONITOR_MODULE_OK;
    CM_SendMsg_1Para(MODULE_SYSMONITOR, MODULE_GPS, MSG_TYPE_COMMAND, CMD_EVENT, CMD_CODE_SYSMONITOR_HEARTBEAT, 0,
        PARA_TYPE_SYSMONITOR_MODULE_STATE,sizeof(U32), (U8*)&rc);   
    MOD_timerRestart(gpsheartBeat, GPS_HB_INTERVAL);
}

ERROR_CODE GPS_decodeDistance(void *h, U8* paraValue, U32 paraLength )
{
    if(paraLength == sizeof(double))
    {
        double dis = *((double*)paraValue);
        if(dis >=0 )
		{
		    *((double*)h) = dis ;
            return PARA_VALUE_ERROR_NO_ERROR;
        }
        else
        {
            return PARA_VALUE_ERROR_INVALID_PARAVALUE;            
        }
    }
    else
    {
        return PARA_VALUE_ERROR_INVALID_PARALENGTH;
    }
}


ERROR_CODE GPS_decodeGPSPosition(void *h, U8* paraValue, U32 paraLength )
{
    GPSPOSPARA* buff = (GPSPOSPARA*)h;

    if(paraLength == sizeof(GPSPOSITION))
    {
		buff->setmask |= 0x01;
		memcpy(&buff->pos,paraValue,sizeof(GPSPOSITION));
        return PARA_VALUE_ERROR_NO_ERROR;
    }
    else
    {
        return PARA_VALUE_ERROR_INVALID_PARALENGTH;
    }
}


/****************************************************
**	
**	open tcp socket on port#4927
**	
****************************************************/
const /*@observer@*/ char *json_error_string(int err)
{
    const char *errors[] = {
	"unknown error while parsing JSON",
	"non-whitespace when expecting object start",
	"non-whitespace when expecting attribute start",
	"unknown attribute name",
	"attribute name too long",
	"saw [ when not expecting array",
	"array element specified, but no [",
	"string value too long",
	"token value too long",
	"garbage while expecting , or }",
	"didn't find expected array start",
	"error while parsing object array",
	"too many array elements",
	"garbage while expecting array comma",
	"unsupported array element type",
	"error while string parsing",
	"check attribute not matched",
	"can't support strings in parallel arrays",
	"invalid enumerated value",
	"saw quoted value when expecting nonstring",
	"didn't see quoted value when expecting string",
	"other data conversion error",
	"unexpected null value or attribute pointer",
    };

    if (err <= 0 || err >= (int)(sizeof(errors) / sizeof(errors[0])))
	    return errors[0];
    else
	    return errors[err];
}

/*
 * We check whether this JSON string is what we need to parse.
 * Otherwise, ignore it simply.
 * iRet == 0 : need parse this json tag 
 * iRet != 0  : ignore this json tag string 
 */
int libgps_json_filter( char *buf)
{
    int status;
    char *classtag = strstr(buf, "\"class\":");

    if (classtag == NULL)
    {
    	//printf("%s: classtag == NULL, return -1\n", __func__);
	    return -1;
    }
#define STARTSWITH(str, prefix)	strncmp(str, prefix, sizeof(prefix)-1)==0

    if (STARTSWITH(classtag, "\"class\":\"TPV\"")) 
    {
    	status = 0; /*need parse this tag*/
		
	//return status;
    }
	else if (STARTSWITH(classtag, "\"class\":\"GST\"")) 
    {
    	status = -1; /*ignore this tag*/
    }
	else if (STARTSWITH(classtag, "\"class\":\"SKY\"")) 
	{
    	status = -1; /*ignore this tag*/
    }
	else if (STARTSWITH(classtag, "\"class\":\"ATT\"")) 
	{
    	status = -1; /*ignore this tag*/
    }
	else if (STARTSWITH(classtag, "\"class\":\"DEVICES\"")) 
	{
    	status = -1; /*ignore this tag*/
    }
	else if (STARTSWITH(classtag, "\"class\":\"DEVICE\"")) 
    {
    	status = 0; /*need parse this tag*/
    }
	else if (STARTSWITH(classtag, "\"class\":\"PPS\"")) 
    {
    	status = -1; /*ignore this tag*/
    }
	else
	{
	    	//printf("%s: classtag is %s, return -1\n", __func__, classtag );
		return -1;
	}
#undef STARTSWITH

    	//printf("%s: classtag is %s, return status = %d\n", __func__, classtag, status );
	return status;
}

/*
 * We should get libgps_dump_state() from the client library, but
 * scons has a bug; we can't get it to add -lgps to the link line,
 * apparently because it doesn't honor parse_flags on a Program()
 * build of a C++ file.
 */
void libgps_dump_state(struct gps_data_t *collect)
{

  if(gps_debug_level <= DEBUG_NONE)
  	return;
  
    /* no need to dump the entire state, this is a sanity check */
#ifndef USE_QT
    /* will fail on a 32-bit macine */
    (void)fprintf(stdout, "flags: (0x%04x) %s\n",
		  (unsigned int)collect->set, gps_maskdump(collect->set));
#endif
    if (collect->set & ONLINE_SET)
        (void)fprintf(stdout, "ONLINE: %lf\n", collect->online);
    if (collect->set & TIME_SET)
        (void)fprintf(stdout, "TIME: time: %lf\n", collect->fix.time);
    if (collect->set & LATLON_SET)
        (void)fprintf(stdout, "LATLON: latitude: %lf  longitude: %lf\n",
		      collect->fix.latitude, collect->fix.longitude);
    if (collect->set & ALTITUDE_SET)
        (void)fprintf(stdout, "ALTITUDE: altitude: %lf  U: climb: %lf\n",
		      collect->fix.altitude, collect->fix.climb);
    if (collect->set & SPEED_SET)
        (void)fprintf(stdout, "SPEED: speed:  %lf\n", collect->fix.speed);
    if (collect->set & TRACK_SET)
        (void)fprintf(stdout, "TRACK: track: %lf\n", collect->fix.track);
    if (collect->set & CLIMB_SET)
        (void)fprintf(stdout, "CLIMB: climb: %lf\n", collect->fix.climb);
    if (collect->set & STATUS_SET) {
        const char *status_values[] = { "NO_FIX", "FIX", "DGPS_FIX" };
        (void)fprintf(stdout, "STATUS: status: %d (%s)\n",
		      collect->status, status_values[collect->status]);
    }
    if (collect->set & MODE_SET) {
        const char *mode_values[] = { "", "NO_FIX", "MODE_2D", "MODE_3D" };
        (void)fprintf(stdout, "MODE: mode: %d (%s)\n",
		      collect->fix.mode, mode_values[collect->fix.mode]);
    }
    if (collect->set & DOP_SET)
        (void)fprintf(stdout,
		      "DOP: satellites %d, pdop=%lf, hdop=%lf, vdop=%lf\n",
		      collect->satellites_used, collect->dop.pdop,
		      collect->dop.hdop, collect->dop.vdop);
    if (collect->set & VERSION_SET)
        (void)fprintf(stdout, "VERSION: release=%s rev=%s proto=%d.%d\n",
		      collect->version.release,
		      collect->version.rev,
		      collect->version.proto_major,
		      collect->version.proto_minor);
    if (collect->set & POLICY_SET)
        (void)fprintf(stdout,
		      "POLICY: watcher=%s nmea=%s raw=%d scaled=%s timing=%s, devpath=%s\n",
		      collect->policy.watcher ? "true" : "false",
		      collect->policy.nmea ? "true" : "false",
		      collect->policy.raw,
		      collect->policy.scaled ? "true" : "false",
		      collect->policy.timing ? "true" : "false",
		      collect->policy.devpath);
    if (collect->set & SATELLITE_SET) {
        int i;

        (void)fprintf(stdout, "SKY: satellites in view: %d\n",
        	      collect->satellites_visible);
	    for (i = 0; i < collect->satellites_visible; i++) {
            (void)fprintf(stdout, "    %2.2d: %2.2d %3.3d %3.0f %c\n",
              collect->PRN[i], collect->elevation[i],
              collect->azimuth[i], collect->ss[i],
              collect->used[i] ? 'Y' : 'N');
        }
    }
    if (collect->set & DEVICE_SET)
        (void)fprintf(stdout, "DEVICE: Device is '%s', driver is '%s'\n",
		      collect->dev.path, collect->dev.driver);
#ifdef OLDSTYLE_ENABLE
    if (collect->set & DEVICEID_SET)
        (void)fprintf(stdout, "GPSD ID is %s\n", collect->dev.subtype);
#endif /* OLDSTYLE_ENABLE */
    if (collect->set & DEVICELIST_SET) {
        int i;
        (void)fprintf(stdout, "DEVICELIST:%d devices:\n",
            collect->devices.ndevices);
        for (i = 0; i < collect->devices.ndevices; i++) {
            (void)fprintf(stdout, "%d: path='%s' driver='%s'\n",
                collect->devices.ndevices,
                collect->devices.list[i].path,
                collect->devices.list[i].driver);
        }
    }
}

int dumpGpsFix( /*@out@*/  struct gps_data_t *data)
/* stuff a fix structure with recognizable out-of-band values */
{
    /*@ -branchstate -compdef @*/
    struct gps_fix_t *fixp = NULL;

    if (!data)
        return -1;

    //gpsdata->set = 0;
    //gpsdata->status = STATUS_NO_FIX;
    //gpsdata->satellites_used = 0;

    //gps_clear_fix(&(data->fix));
    fixp = &(data->fix);

    printf(" GPS reading ..\n");                                 
    printf(" ----------------------------------------\n");                                 

    printf(" #latitude (deg)	  =%f, 	epy=%f \n", fixp->latitude, fixp->epy );
    printf(" #longitude (deg) =%f, 	epx=%f \n", fixp->longitude, fixp->epx );
    printf(" #altitude (m)	  =%f, 	epv=%f \n", fixp->altitude, fixp->epv );
    printf(" #time utc		  =%f, 	ept=%f, 	#mode=%d \n", fixp->time, fixp->ept, fixp->mode );

    printf(" #track		  =%f, 	epd=%f \n", fixp->track, fixp->epd );
    printf(" #speed  (m/s)	  =%f, 	eps=%f \n", fixp->speed, fixp->eps );
    printf(" #climb		  =%f, 	epc=%f \n", fixp->climb, fixp->epc );

    //printf(" #satelites		  =%f, 	epc=%f \n", fixp->climb, fixp->epc );
    printf(" ----------------------------------------\n");                              

    //printf("END OF GPSCTRL DUMP\n\n");                                 

	return 0;

}
int speed_arr[] = {B115200, B38400, B19200, B9600, B4800, B2400, B1200, B300,  
                   B115200, B38400, B19200, B9600, B4800, B2400, B1200, B300, };  
int name_arr[] = {115200, 38400, 19200, 9600, 4800, 2400, 1200,  300,   
                  115200, 38400, 19200, 9600, 4800, 2400, 1200,  300, };  
void set_speed(int fd, int speed){  
  int   i;   
  int   status;   
  struct termios   Opt;  
  tcgetattr(fd, &Opt);   
  for ( i= 0;  i < sizeof(speed_arr) / sizeof(int);  i++) {   
    if  (speed == name_arr[i]) {       
      tcflush(fd, TCIOFLUSH);       
      cfsetispeed(&Opt, speed_arr[i]);    
      cfsetospeed(&Opt, speed_arr[i]);     
      status = tcsetattr(fd, TCSANOW, &Opt);    
      if  (status != 0) {          
        perror("tcsetattr fd1");    
        return;       
      }      
      tcflush(fd,TCIOFLUSH);     
    }    
  }  
} 
int set_Parity(int fd,int databits,int stopbits,int parity)  
{   
    struct termios options;   
    if  ( tcgetattr( fd,&options)  !=  0) {   
        perror("SetupSerial 1");       
        return(FALSE);    
    }  
    options.c_cflag &= ~CSIZE;   
    switch (databits) /*ÉèÖÃÊý¾ÝÎ»Êý*/  
    {     
    case 7:       
        options.c_cflag |= CS7;   
        break;  
    case 8:       
        options.c_cflag |= CS8;  
        break;     
    default:      
        fprintf(stderr,"Unsupported data size\n"); return (FALSE);    
    }  
    switch (parity)   
    {     
        case 'n':  
        case 'N':      
            options.c_cflag &= ~PARENB;   /* Clear parity enable */  
            options.c_iflag &= ~INPCK;     /* Enable parity checking */   
            break;    
        case 'o':     
        case 'O':       
            options.c_cflag |= (PARODD | PARENB); /* ÉèÖÃÎªÆæÐ§Ñé*/    
            options.c_iflag |= INPCK;             /* Disnable parity checking */   
            break;    
        case 'e':    
        case 'E':     
            options.c_cflag |= PARENB;     /* Enable parity */      
            options.c_cflag &= ~PARODD;   /* ×ª»»ÎªÅ¼Ð§Ñé*/       
            options.c_iflag |= INPCK;       /* Disnable parity checking */  
            break;  
        case 'S':   
        case 's':  /*as no parity*/     
            options.c_cflag &= ~PARENB;  
            options.c_cflag &= ~CSTOPB;break;    
        default:     
            fprintf(stderr,"Unsupported parity\n");      
            return (FALSE);    
        }    
    /* ÉèÖÃÍ£Ö¹Î»*/    
    switch (stopbits)  
    {     
        case 1:      
            options.c_cflag &= ~CSTOPB;    
            break;    
        case 2:      
            options.c_cflag |= CSTOPB;    
           break;  
        default:      
             fprintf(stderr,"Unsupported stop bits\n");    
             return (FALSE);   
    }   
    /* Set input parity option */   
    if (parity != 'n')     
        options.c_iflag |= INPCK;   
    tcflush(fd,TCIFLUSH);  
    options.c_cc[VTIME] = 150; /* ÉèÖÃ³¬Ê±15 seconds*/     
    options.c_cc[VMIN] = 0; /* Update the options and do it NOW */  
    if (tcsetattr(fd,TCSANOW,&options) != 0)     
    {   
        perror("SetupSerial 3");     
        return (FALSE);    
    }   
    options.c_lflag  &= ~(ICANON | ECHO | ECHOE | ISIG);  /*Input*/  
    options.c_oflag  &= ~OPOST;   /*Output*/  
    return (TRUE);    
}  
gps_ret_t GPS_initCom()
{
    unsigned int flags;
	struct fixsource_t source;
	/* enable logging level inside libgps.so */
    gps_enable_debug(gps_debug_level, stdout);
	
    /* Grok the server, port, and device. */
    if ( strlen(zGpsdSource)>0 )
		gpsd_source_spec(zGpsdSource, &source);
    else
		gpsd_source_spec(NULL, &source);

    /*@ -nullpass -onlytrans @*/
    if (gps_open(source.server, source.port, &gpsdata) != 0) 
	{
		
		//(void)fprintf(stderr,
		//	      "gpsctrl: could not connect to gpsd %s:%s, %s(%d)\n",
		//	      source.server, source.port, strerror(errno), errno);

    	GPSLogWarning("could not connect to gpsd %s:%s, %s(%d)", 
			 source.server, source.port, strerror(errno), errno);

		return gps_open_fail;
    } 
	else 
	{
    	GPSLogInfo("succeed connect to gpsd %s:%s", source.server, source.port);
	}
	/*@ +nullpass +onlytrans @*/

    /*@-branchstate@*/
    flags = WATCH_ENABLE;
	//    flags |= WATCH_NMEA;
	//    flags |= WATCH_RAW;
    flags |= WATCH_JSON;
	//	    flags |= WATCH_PPS;

	/* send starting stream to query JSON results */
    (void)gps_stream(&gpsdata, flags, source.device);
	return gps_ret_ok;
	
}
bool isRMC(uint8_t *gpsData)
{                                             
    bool status = false;                                
    if(*(gpsData + 2) == 'R' && *(gpsData + 3) == 'M' && *(gpsData + 4) == 'C')
    {
       status = true;
    }
                                     
    return status;                               
}  
bool isGGA(uint8_t *gpsData)
{                                             
    bool status = false;                                
    if(*(gpsData + 2) == 'G' && *(gpsData + 3) == 'G' && *(gpsData + 4) == 'A')
    {
       status = true;
    }
                                     
    return status;                               
}  
uint16_t field( uint8_t desired_field_number, uint8_t rmc_field[], uint8_t *gpsData) 
{
    uint8_t return_string[RMC_MAX_BUF];
    int32_t i;
    int32_t index                = 0; 
    int32_t current_field_number = 0;
    int32_t string_length        = 0;
    
    memset(return_string, 0, RMC_MAX_BUF);
    string_length = GPS_RECV_CMD_MAX_BUF;
    
    while( current_field_number < desired_field_number && index < string_length )
    {
        if ( *(gpsData + index)  == ',' || *(gpsData + index) == '*' )
        {
            current_field_number++;
        }
        
        index++;
    }
    
    if ( current_field_number == desired_field_number )
    {
       i = 0;
       while( index < string_length  &&
              *(gpsData + index) != ',' &&
              *(gpsData + index) != '*' &&
              *(gpsData + index) != 0x00 )
       {
           return_string[i] = *(gpsData + index);
           index++;
           i++;
       }
    }
    
    for(index = 0; index < i; index++)
    {
        rmc_field[index] = return_string[index];
    }
    
    return 0;
}
void reportGpsAlarm(uint32_t alarm)
{
    char msg_buf[MSG_BUFF_SIZE] = {0};
    MSGHEADER *msg_header = (MSGHEADER *)msg_buf;
    void* msg_payload = (void*)(msg_buf + sizeof(MSGHEADER));

    msg_header->srcID = MODULE_GPS;
    msg_header->dstID = MODULE_INTF;
    msg_header->mType = get_mtype(msg_header->dstID);
    msg_header->msgType = MSG_TYPE_COMMAND;

    PCMDHEADER cmd = (PCMDHEADER)msg_payload;
    cmd->cmdType = CMD_EVENT;
    cmd->cmdCode = alarm;
    cmd->cmdIndex = 0;
    cmd->cmdLength = 0;

    msg_header->msgLength = sizeof(CMDHEADER) + sizeof(MSGHEADER);

    if( RC_OK != CM_sendMsg(msg_header, msg_payload) )
    {
        GPSLogWarning(" can't report msg \n");
    }
} 

bool isDataValid(uint8_t field_number, uint8_t *gpsData) 
{
    uint8_t field_data[RMC_MAX_BUF] = {0};
    memset(field_data, 0, RMC_MAX_BUF);
    
    field( field_number, field_data, gpsData);
    
    if ( *field_data == 'A' )
    {
        return( true );
    }
    else
    {
        gpsdata.status = 0;
        reportGpsAlarm(CMD_EVT_ALARM_GPS_LOST);
        return( false );
    }
}
bool isNorth(uint8_t field_number, uint8_t *gpsData) 
{
    uint8_t field_data[RMC_MAX_BUF] = {0};
    memset(field_data, 0, RMC_MAX_BUF);
    
    field( field_number, field_data, gpsData);
    
    if ( *field_data == 'N' )
    {
        return( true );
    }
    else
    {
        return( false );
    }
}
bool isEast(uint8_t field_number, uint8_t *gpsData) 
{
    uint8_t field_data[RMC_MAX_BUF] = {0};
    memset(field_data, 0, RMC_MAX_BUF);
    
    field( field_number, field_data, gpsData);
    
    if ( *field_data == 'E' )
    {
        return( true );
    }
    else
    {
        return( false );
    }
}
uint64_t decodeGpsRmc(uint8_t *gpsData)
{
    //rmc_info_t rmcinfo;
    struct tm utcTime;
    struct tm gpsStart;
    
    //gps time start from 1980,Jan-6, 0 hour
    gpsStart.tm_year = 80;
    gpsStart.tm_mon = 0;
    gpsStart.tm_mday = 6;
    gpsStart.tm_hour = 0;
    gpsStart.tm_min = 0;
    gpsStart.tm_sec = 0;

    uint8_t llatitude[RMC_MAX_BUF];
    uint8_t llongitude[RMC_MAX_BUF];
    uint8_t allitude[RMC_MAX_BUF];
    //uint8_t ddegree[4];
    //uint8_t mminute[8];
    uint8_t ttime[RMC_MAX_BUF];
    uint8_t hhour[4];
    uint8_t mminute[4];
    uint8_t ssecond[4];
    uint8_t ddate[RMC_MAX_BUF];
    uint8_t dday[4];
    uint8_t mmonth[4];
    uint8_t yyear[4];
    uint8_t i = 0;
    uint64_t start;
    uint64_t end;

    uint8_t  *rmcData = NULL;

    for (i = 0; i < GPS_RECV_CMD_MAX_BUF; i++)
    {
        if(*(gpsData + i) == '$')
        {
            rmcData = gpsData + i + 1;
            break;
        } 
    }

    if(rmcData == NULL) return 0;
    //proc the RMC info
    if(isRMC(rmcData)) 
    {
      if(isDataValid(2, rmcData)){
        
        //is valid RMC info
        gpsdata.status = 1;

        //Time:6-15
        memset(ttime, 0, RMC_MAX_BUF);
        field(1, ttime, rmcData);
     
        for(i = 0;i < 4;i++)
          hhour[i]=0;

        hhour[0]=ttime[0];
        hhour[1]=ttime[1];
    
        for(i = 0;i < 4;i++)
          mminute[i]=0;
          
        mminute[0]=ttime[2];
        mminute[1]=ttime[3];
     
        for(i = 0;i < 4;i++)
           ssecond[i]=0;
     
        ssecond[0]=ttime[4];
        ssecond[1]=ttime[5];
     
        utcTime.tm_hour = atoi((const char *)hhour);
        utcTime.tm_min = atoi((const char *)mminute);
        utcTime.tm_sec = atoi((const char *)ssecond);

        //latitude
        memset(llatitude, 0, RMC_MAX_BUF);
        field(3, llatitude, rmcData);

        if(isNorth(4, rmcData))
        {
          gpsdata.fix.latitude = (double)(atof((const char*)llatitude)*0.01);
        }
        else
        {
            gpsdata.fix.latitude = (double)(atof((const char*)llatitude) * (-0.01));
        }

        printf("--------------------gps_latitude=%f-----------\n\n",gpsdata.fix.latitude);  
        //longtitude
        memset(llongitude, 0, RMC_MAX_BUF);
        field(5, llongitude, rmcData);
        if(isEast(6, rmcData))
        {
            gpsdata.fix.longitude = (double)(atof((const char*)llongitude)*0.01);
        }
        else
        {
            gpsdata.fix.longitude = (double)(atof((const char*)llongitude) * (-0.01));
        }
    
        printf("--------------------gps_longitude=%f-----------\n\n",gpsdata.fix.longitude);  
        //Date:
        memset(ddate, 0, RMC_MAX_BUF);
        field(9, ddate, rmcData);
        
        for(i = 0;i < 4;i++)
          dday[i]=0;
          
        dday[0]=ddate[0];
        dday[1]=ddate[1];
    
        for(i = 0;i < 4;i++)
          mmonth[i]=0;
     
        mmonth[0]=ddate[2];
        mmonth[1]=ddate[3];
     
        for(i = 0;i < 4;i++)
          yyear[i]=0;
     
        yyear[0]=ddate[4];
        yyear[1]=ddate[5];
     
        utcTime.tm_mday = atoi((const char *)dday);
        utcTime.tm_mon = atoi((const char *)mmonth) - 1;
        utcTime.tm_year = atoi((const char *)yyear) + 100;

        start = (uint64_t)mktime(&utcTime);
        end = (uint64_t)mktime(&gpsStart);

        printf("utcTime = %llu, gpsStart = %llu \n", start, end);
        //workaround since can not get gps time, and gps is ahead of UTC time by 17 seconds
        return (start - end + 17);
      }
    } //end  isRMC
    else if(isGGA(rmcData))
    {
        memset(allitude, 0, RMC_MAX_BUF);
        field(9, allitude, rmcData);
        gpsdata.fix.altitude = (double)(atof((const char*)allitude));
        printf("--------------------gps_allitude=%f-----------\n\n",gpsdata.fix.altitude);  
    }
    return 0;
}
 


void GPS_setProc(PCMDHEADER pCmd, PCMDHEADER pAck)
{
    switch(pCmd->cmdCode)
    {
        case CMD_CODE_GPS_EXPECTED_POSITION:
			{
				GPSPOSPARA posPara={0};
				if(RC_OK == CM_paraListDecoder(pCmd,pAck, gpsPostionParaTab,sizeof(gpsPostionParaTab)/sizeof(PARADECODERMAP),&posPara,FALSE))
				{
					if((posPara.setmask&0x01)== 0)
					{
						pAck->cmdLength = CM_paraPackError((PPARAM)(pAck+1),PARA_VALUE_ERROR_NOT_ENOUGH_PARA);

					}
					else
					{
						expectedPosition = posPara.pos;
                        expectedPosition_set = TRUE;
						pAck->cmdLength = CM_paraPackU32((PPARAM)(pAck+1),PARA_TYPE_RESULT,PARA_VALUE_RESULT_SUCCESS);
					}
				}
			}
			break;
		case CMD_CODE_GPS_ALERT_THRESHOLD:
            {
                double dis = -1;
                if(RC_OK == CM_paraListDecoder(pCmd,pAck, gpsDistanceTab,sizeof(gpsDistanceTab)/sizeof(PARADECODERMAP),&dis,FALSE))
                {
                    if(dis != -1)
                    {
                        gps_alert_threshold = dis;
						pAck->cmdLength = CM_paraPackU32((PPARAM)(pAck+1),PARA_TYPE_RESULT,PARA_VALUE_RESULT_SUCCESS);
                    }
                    else
                    {
						pAck->cmdLength = CM_paraPackError((PPARAM)(pAck+1),PARA_VALUE_ERROR_NOT_ENOUGH_PARA);
                    }
                }
            }
            break;

//here is test heartbeat
        case CMD_CODE_SYSMONITOR_TEST_MODULE:
            GPSLogError("----- test module gps ----- %04x", CMD_CODE_SYSMONITOR_TEST_MODULE);
            while(1)
            {

            }
            break;
     
    
        default:
            pAck->cmdLength = CM_paraPackError((PPARAM)(pAck+1), PARA_VALUE_ERROR_UNKNOWN_CMDCODE);
            GPSLogError("Unknown SET cmd cmdCode=%04x",pCmd->cmdCode);
            break;
    }

}

void GPS_getProc(PCMDHEADER pCmd, PCMDHEADER pAck)
{
    switch(pCmd->cmdCode)
    {
        case CMD_CODE_GPS_STATUS:
			{
                PPARAM pParam = (PPARAM)(pAck + 1);
                U8 *pValue = NULL;
				
                pAck->cmdLength +=CM_paraPackPara(&pParam, PARA_TYPE_GPS_STATUS, sizeof(U32),&pValue);
                memcpy(pValue,&gpsStatus, sizeof(U32));

                pAck->cmdLength +=CM_paraPackPara(&pParam, PARA_TYPE_GPS_COORDINATE, sizeof(GPSPOSITION),&pValue);
                memcpy(pValue,&currentPosition, sizeof(GPSPOSITION));


			}
			break;
        default:
            pAck->cmdLength = CM_paraPackError((PPARAM)(pAck+1), PARA_VALUE_ERROR_UNKNOWN_CMDCODE);
            GPSLogError("Unknown SET cmd cmdCode=%04x",pCmd->cmdCode);
            break;
    }

}

void GPS_eventProc(PCMDHEADER pCmd, PCMDHEADER pAck)
{
    switch(pCmd->cmdCode)
    {
        case CMD_EVT_ALARM_PM_GRAND_LOST:
			{
                GPSLogNotice("device grand lost so report gps info to fep");
                reportGps = true;
			}
			break;
        default:
            pAck->cmdLength = CM_paraPackError((PPARAM)(pAck+1), PARA_VALUE_ERROR_UNKNOWN_CMDCODE);
            GPSLogError("Unknown SET cmd cmdCode=%04x",pCmd->cmdCode);
            break;
    }

}


int OpenDev(char *Dev)
{

    //Dev ¾ÍÊÇÉè±¸£¬Éè±¸¾ÍÊÇÎÄ¼þ£¬¾ÍÊÇ¸ø³ö¸ÃÉè±¸ÎÄ¼þµÄÂ·¾¶

    int fd = open(Dev, O_RDWR ); //| O_NOCTTY | O_NDELAY

    if (-1 == fd)

    {

       GPSLogError("Can't Open Serial Port");

       return -1;

    }

    else

       return fd;

}
#ifndef GPS
gps_ret_t GPS_Proc()
{
    char buf[4096];
    char jsonstr[4096];

    fd_set fds;	
	int r = 0;
	struct timeval tv;

	tv.tv_sec = 0;
	tv.tv_usec = 200000;

	FD_ZERO(&fds);
	FD_SET(gpsdata.gps_fd, &fds);
	
	while(TRUE)
	{
		errno = 0;
		r = select(gpsdata.gps_fd+1, &fds, NULL, NULL, &tv);
		if (r == -1 ) 
		{
			if(errno == EINTR)
			{
				continue;
			}
			else
	    	{	
	    		GPSLogWarning("select error %s(%d)", strerror(errno), errno);
				return gps_select_fail;
			} 
		} 
		else if (r == 0)
		{
			//newloop ++;
	        //printf("gpsctrl [%s]: select (r == 0) timeout. every newloop=%d\n", __func__, newloop );
			// if(newloop==1)	dumpGpsFix(&gpsdata); 
			GPSLogDebug("select timeout");
			return gps_select_timeout;
		}
		break;
	}
	
	//printf("gpsctrl [%s]: select (r > 0). newloop=%d\n", __func__, newloop );
	//newloop = 0; //clear it 
	
	//reading directly from the socket avoids decode overhead 
	while(TRUE)
	{
		errno = 0;
		r = (int)read(gpsdata.gps_fd, buf, sizeof(buf));

		if (r == -1) 
		{	
			if (errno == EINTR)
				continue;
			else 
			{
				GPSLogWarning("read error %s(%d)", strerror(errno), errno);
				
				return gps_read_fail;
			}
		} 
		break;
	}
	
	if (r > 0) 
	{
	    int  i = 0;
	    int  jsonlen=0;
	
	    for (i = 0; i < r; i++) 
	    {
			char c = buf[i];
			jsonlen++;
			

			//GPSLogDebug("%c",c);
			
			if (c == '\n') 
			{
				//parse gps stream to update local &(data->fix) object 
			    int status = 0;
					
				//jsonstr[0] = '\0';
                //GPSLogDebug("%s", buf+(i+1-jsonlen));
				(void)strncpy( (char *)jsonstr, buf+(i+1-jsonlen), jsonlen);
				jsonstr[jsonlen] = '\0';
				if ( libgps_json_filter(jsonstr) == 0)
				{
					//printf(" *** DECODE JSON STRING=\n%s\n", jsonstr );
					GPSLogInfo("*** DECODE JSON STRING=\n%s", jsonstr);
				    status = gps_unpack( jsonstr, &gpsdata);

					//status = libgps_json_unpack(jsonstr, &gpsdata, NULL);
					if (status != 0) 
					{
				        GPSLogWarning("decode error status %d (%s) ", status, json_error_string(status) );
						return gps_decode_fail;
					}
					else
						libgps_dump_state(&gpsdata);
				}
				//reset json str 
                memset(jsonstr, 0, jsonlen);
				jsonlen = 0;
				
			}
		
	    }
		
	}
	else
	{
		return gps_read_nodata;
	}

	return gps_ret_ok;
}

#else
gps_ret_t GPS_Proc()
{

    int    fd, c=0, res;  
    U8* tmp;
    char  buf[155];  
  
    fd = open("/dev/ttyS2", O_RDWR | O_NOCTTY |O_NDELAY);  
  
    if (fd < 0) {  
        perror("/dev/ttyS2");
        exit(1);  
    }  
  
    set_speed(fd,9600);  
    if (set_Parity(fd,8,1,'N') == FALSE)  {  
        GPSLogError("Set Parity Error\n");  
        exit (0);  
    } 
	
    while(1) 
	{  
        res = read(fd, buf, 155);
        if(res<0)
        {
            if(gps_flag == 0)
            {
                gps_flag = 1;
		        system("/etc/init.d/gpsd restart");
			    sleep(1);
		        system("echo gpsd restart completes");
            }
            
            break;
        }  
        if(res==0)
		{
            return gps_read_fail;
        }  
		
        if(buf[0]==0x0d) printf("\n");

		decodeGpsRmc(buf);
        if(tmp=strstr(buf,"GGA"))
		{
            decodeGpsRmc(tmp);
            break; 
        }
    }  
    close(fd);  
  
    return 0;
}
#endif	
 
void GPS_timer(void *h)
{
	gps_ret_t rc = gps_ret_ok;

	MOD_timerRestart(gpsTimer,GPS_TIMER_INTERVAL);

	if(!gpsStarted)
	{
		if(gps_ret_ok != GPS_initCom())
		{
			GPSLogWarning("[SeqNo %d] Can't initialize GPS communication with GPSD Service", gps_retry_num);

	 		if( gps_retry_num ++ >=  GPS_RETRY_MAX )
			{
				gps_retry_num = 0;	// re-init

				GPSLogWarning("Need Restart GPSD Service Now");

				// add protction to re-start gpsd service
				//sprintf(cmd, "/etc/init.d/gpsd restart");
				system("/etc/init.d/gpsd restart");

				sleep(1);
				system("echo gpsd restart completes");
			}
			
			return;
		}
		
		gps_retry_num = 0;	// reset

		gpsStarted = TRUE;

		/* return directly, and process real data in next loop */
		return;
	}

	rc= GPS_Proc();

	if(gps_ret_ok != rc)
	{
		GPSLogDebug("[SeqNo %d] ERROR GPS_Proc() rc = %d", gps_continue_nodata_cnt, rc );
	
		if (gps_continue_nodata_cnt ++ >= GPS_NODATA_THRESHOLD )
		{
			GPSLogInfo("%d times ERR happen, GPSD service may halt, GPS_Proc() rc = %d", GPS_NODATA_THRESHOLD, rc );

			gps_close(&gpsdata);

			/*switch state & clear up old DATA */
			gpsStarted = FALSE;

			memset(&gpsdata, 0, sizeof(gpsdata));
			GPS_getStatus(&gpsStatus,&currentPosition);		

			return;				
		}
	}
			
	if(gps_select_fail == rc || gps_read_fail == rc )
	{	
		gps_close(&gpsdata);
        GPSLogDebug("can not read from gpsd");
		/*switch state & clear up old DATA */
		gpsStarted = FALSE;
		//memset(&gpsdata, 0, sizeof(gpsdata));
		//GPS_getStatus(&gpsStatus,&currentPosition);
		return;	
	}
	else if(gps_ret_ok == rc)
	{
		/* clear error statustics cnt */
		gps_continue_nodata_cnt = 0;
		
		GPS_getStatus(&gpsStatus,&currentPosition);

		if(gpsStatus == GPS_ONLINE)
		{
			CM_SendMsg_2Para(MODULE_SYSTEM,MODULE_GPS,
				MSG_TYPE_COMMAND,CMD_EVENT,CMD_CODE_GPS_STATUS,gCmdIndex++,
				PARA_TYPE_GPS_STATUS,sizeof(gpsStatus),(U8*)&gpsStatus,
				PARA_TYPE_GPS_COORDINATE,sizeof(currentPosition),(U8*)&currentPosition);
            GPSLogInfo("longitude:%f latitude:%f altitude:%f", currentPosition.longitude, currentPosition.latitude, currentPosition.altitude);

            if (reportGps)
            {
                CM_SendMsg_2Para(MODULE_INTF,MODULE_GPS,
                        MSG_TYPE_COMMAND,CMD_EVENT,CMD_CODE_GPS_ALERT_INFO,gCmdIndex++,
                        PARA_TYPE_GPS_STATUS,sizeof(gpsStatus),(U8*)&gpsStatus,
                        PARA_TYPE_GPS_COORDINATE,sizeof(currentPosition),(U8*)&currentPosition);
                GPSLogNotice("report gps info to fep because grand lost");
            }
            if(expectedPosition_set)
            {
                double dis = GPS_getDistance(&expectedPosition,&currentPosition);
                if( dis >= gps_alert_threshold)
                {
                    
                    CM_SendMsg_2Para(MODULE_INTF,MODULE_GPS,
                        MSG_TYPE_COMMAND,CMD_EVENT,CMD_CODE_GPS_ALERT_MOVE,gCmdIndex++,
                        PARA_TYPE_GPS_COORDINATE,sizeof(currentPosition),(U8*)&currentPosition,
                        PARA_TYPE_GPS_DISTANCE,sizeof(double),(U8*)&dis
                        );
                }

            }

		}
		else
		{
			CM_SendMsg_1Para(MODULE_SYSTEM,MODULE_GPS,
				MSG_TYPE_COMMAND,CMD_EVENT,CMD_CODE_GPS_STATUS,gCmdIndex++,
				PARA_TYPE_GPS_STATUS,sizeof(gpsStatus),(U8*)&gpsStatus);

		}
		
	}

}


/*****************************************************
** GPS Task Main Responsibility
**	
** 	1 	listen to tcp.port_2947 @ localhost from GPSD Service per 1sec;
**	2 	Record the GPS result into a global System Variable;
**  	3 	DEV_manage read from the global System Variable if needed;
**	4 	DEV_manage decide whether the gps position moved or not ??

*****************************************************/
static void* GPS_main(void *arg __attribute__((unused)))
{
    pthread_setname_np(pthread_self(), "gpsctrl");

	CMMODINFO cmModInfo = {0};

    gpsTimer= MOD_timerStart(MODULE_GPS,GPS_TIMER_INTERVAL,GPS_timer,NULL);
    gpsheartBeat = MOD_timerStart(MODULE_GPS,GPS_HB_INTERVAL,reportGpsHeartBeat,NULL);


    cmModInfo.ModID = MODULE_GPS;
    cmModInfo.useTimer = TRUE;
    cmModInfo.setProc = GPS_setProc;
    cmModInfo.getProc = GPS_getProc;
    cmModInfo.eventProc = GPS_eventProc;
    CM_cmdDispatcher(&cmModInfo);
   
    GPSLogNotice("gps thread exit \n");
    pthread_exit((void*)0);
}

RESULT GPS_init()
{
    pthread_t gps_thread;

    if( RC_OK != MOD_register(&modGpsInfo) )
    {
        GPSLogError("Can't register gps module with syscore\n");
        return RC_FAIL;
    }

    pthread_create(&gps_thread, NULL, GPS_main, 0);

    return RC_OK;
}


void GPS_getStatus(U32 *status, GPSPOSITION* pos)
{
    if(gpsdata.status > 0)
    {
		*status = GPS_ONLINE;
		pos->longitude = gpsdata.fix.longitude;
		pos->latitude = gpsdata.fix.latitude;
		pos->altitude = gpsdata.fix.altitude;

    }
    else
    {
		*status = GPS_OFFLINE;
		pos->longitude = gpsdata.fix.longitude;
		pos->latitude = gpsdata.fix.latitude;
		pos->altitude = gpsdata.fix.altitude;
    }   
}

double GPS_getDistance(GPSPOSITION* pos1,GPSPOSITION* pos2)
{
	/*
		discard altidude becasue its bad accurateness. 
		
		d = R * acos( cos(lat1)*cos(lat2)*cos(lon2-lon1) + sin(lat1) * sin(lat2) )
		
	*/

	
	double R = 6371004;  // m
	double pi = 3.1415916;
	double lon1 = pos1->longitude* pi / 180;
    double lat1 = pos1->latitude * pi / 180;
    double lon2 = pos2->longitude* pi / 180;
    double lat2 = pos2->latitude * pi / 180;

	double d = R * acos( cos(lat1)*cos(lat2)*cos(lon2-lon1) + sin(lat1) * sin(lat2));

	return d;
}
